﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using MeshSharp.Core.Models;

namespace MeshSharp.Core.Utilities
{
    public static class MathUtils
    {
        public static Vector3 MidPoint(Vector3 v1, Vector3 v2)
        {
            return new Vector3((v1.X+v2.X)/2, (v1.Y+v2.Y)/2, (v1.Z+v2.Z)/2);
        }

        public static double Distance(Vector3 v1, Vector3 v2)
        {
            return Math.Sqrt(Math.Pow((v2.X - v1.X), 2) + Math.Pow((v2.Y - v1.Y), 2) + Math.Pow((v2.Z - v1.Z), 2));
        }

        public static double DotProduct(Vector3 v1, Vector3 v2)
        {
            return (v1.X*v2.X) + (v1.Y*v2.Y) + (v1.Z*v2.Z);
        }

        public static Vector3 CrossProduct(Vector3 v1, Vector3 v2)
        {
            return new Vector3(v1.Y * v2.Z - v1.Z * v2.Y, v1.Z * v2.X - v1.X * v2.Z, v1.X * v2.Y - v1.Y * v2.X);
        }

        public static bool PointIsOnLine(Vector3 point, Vector3 lineVertex1, Vector3 lineVertex2)
        {
            return (Math.Min(lineVertex1.X, lineVertex2.X) <= point.X 
                    && point.X <= Math.Max(lineVertex1.X, lineVertex2.X)
                    && Math.Min(lineVertex1.Y, lineVertex2.Y) <= point.Y
                    && point.Y <= Math.Max(lineVertex1.Y, lineVertex2.Y));
        }

        public static Vector3 Intersect(Vector3 line1V1, Vector3 line1V2, Vector3 line2V1, Vector3 line2V2)
        {
            //Line1
            float A1 = line1V2.Y - line1V1.Y;
            float B1 = line1V1.X - line1V2.X;
            float C1 = A1*line1V1.X + B1*line1V1.Y;

            //Line2
            float A2 = line2V2.Y - line2V1.Y;
            float B2 = line2V1.X - line2V2.X;
            float C2 = A2 * line2V1.X + B2 * line2V1.Y;

            float det = A1*B2 - A2*B1;
            if (det == 0)
            {
                return null;//parallel lines
            }
            else
            {
                float x = (B2*C1 - B1*C2)/det;
                float y = (A1 * C2 - A2 * C1) / det;
                return new Vector3(x,y,0);
            }
        }

        /// <summary>
        /// Check if the vector falls withing the triangle using the BaryCentric formula.
        /// </summary>
        /// <param name="triIndices"></param>
        /// <param name="vec"></param>
        /// <returns></returns>
        internal static bool IsInTriangle(Vector3[] triIndices, Vector3 point)
        {
            // Compute vectors        
            Vector3 v0 = triIndices[2] - triIndices[0];
            Vector3 v1 = triIndices[1] - triIndices[0];
            Vector3 v2 = point - triIndices[0];

            // Compute dot products
            double dot00 = DotProduct(v0, v0);
            double dot01 = DotProduct(v0, v1);
            double dot02 = DotProduct(v0, v2);
            double dot11 = DotProduct(v1, v1);
            double dot12 = DotProduct(v1, v2);

            // Compute barycentric coordinates
            double invDenom = 1/(dot00*dot11 - dot01*dot01);
            double u = (dot11*dot02 - dot01*dot12)*invDenom;
            double v = (dot00*dot12 - dot01*dot02)*invDenom;     

            // Check if point is in triangle
            return (u > 0) && (v > 0) && (u + v < 1);
        }

    }
}
